#pragma once

#include "main.h"
#include "units.h"
#include <arm_math.h>

using float32_t = float;

#ifndef PI
constexpr float32_t PI = 3.1415926535897932384626433832795f;
#endif

namespace os::math {
using namespace units::angle;
void sinCos(radian_t angle, float32_t* pSin, float32_t* pCos);
radian_t atan2(float32_t y, float32_t x);
radian_t asin(float32_t x);
void abs(float32_t* src, float32_t* dst, uint32_t size);
void add(float32_t* src_a, float32_t* src_b, float32_t* dst, uint32_t size);
void sub(float32_t* src_a, float32_t* src_b, float32_t* dst, uint32_t size);
float32_t cos(radian_t angle);
float32_t sin(radian_t angle);
float32_t tan(radian_t angle);
radian_t atan(float32_t x);
float32_t invSqrt(float32_t x);
int8_t sign(float32_t x);
float32_t sqrt(float32_t x);
int16_t buff2i16(uint8_t const* buff);
float buff2float(uint8_t const* buff);
void ui322buff(uint32_t u, uint8_t* buff);

template <typename T>
T normalize_angle(T const angle, T const min_angle, T const max_angle) {
    T width = max_angle - min_angle;
    T angle_out = angle;
    while (angle_out <= min_angle) {
        angle_out += width;
    }
    while (angle_out > max_angle) {
        angle_out -= width;
    }
    return angle_out;
}
}  // namespace os::math

namespace os {
namespace math {
#ifdef MathErrorLoop
[[noreturn]] void MathErrorHandler(char const* msg);
#else
void MathErrorHandler(char const* msg);
#endif
}  // namespace math
}  // namespace os

/**
 * @brief      Limit function
 * @param      input :Limited value
 * @param      max :Max limite value
 * @retval     NULL
 */
#define LimitMax(input, max)       \
    {                              \
        if (input > max) {         \
            input = max;           \
        } else if (input < -max) { \
            input = -max;          \
        }                          \
    }

/**
 * @brief      Maximum and minimum limiting
 * @param      input :Limited value
 * @param      max :Max limite value
 * @param      min :Min limite value
 * @retval     NULL
 */
#define LimitMaxMin(input, max, min) \
    {                                \
        if (input > max) {           \
            input = max;             \
        } else if (input < min) {    \
            input = min;             \
        }                            \
    }

#define GetMaxandMinRange(input, max, min) \
    {                                      \
        if (input >= max) {                \
            max = input;                   \
        } else if (input <= min) {         \
            min = input;                   \
        }                                  \
    }

#ifndef MIN
#define MIN(a, b) ((a) < (b) ? (a) : (b))
#endif

#ifndef MAX
#define MAX(a, b) ((a) > (b) ? (a) : (b))
#endif

namespace os::math {
/**
 * @brief      Slope control class for smooth acceleration/deceleration
 */
class SlopeCtrl {
private:
    float acc_;  // Acceleration rate
    float dec_;  // Deceleration rate

public:
    /**
     * @brief      Constructor with acceleration and deceleration parameters
     * @param      acc: Acceleration rate
     * @param      dec: Deceleration rate
     */
    explicit SlopeCtrl(float const acc = 0.0f, float const dec = 0.0f) : acc_(acc), dec_(dec) {}

    /**
     * @brief      Set slope parameters
     * @param      acc: Acceleration rate
     * @param      dec: Deceleration rate
     * @retval     None
     */
    void setParams(float const acc, float const dec) {
        acc_ = acc;
        dec_ = dec;
    }

    /**
     * @brief      Get acceleration rate
     * @retval     Current acceleration rate
     */
    float getAcc() const { return acc_; }

    /**
     * @brief      Get deceleration rate
     * @retval     Current deceleration rate
     */
    float getDec() const { return dec_; }

    /**
     * @brief      Calculate slope function setting
     * @param      rawref: Current setting value
     * @param      targetref: Target set point
     * @retval     Slope function setting value. If slope function is not enabled (parameters are 0), the target setting
     * value is returned
     */
    float calcRef(float rawref, float targetref) const {
        if (acc_ == 0.0f || dec_ == 0.0f) {
            return targetref;
        }

        if (rawref < targetref - acc_) {
            return rawref + acc_;
        }
        if (rawref > targetref + dec_) {
            return rawref - dec_;
        }
        return targetref;
    }

    /**
     * @brief      Calculate the absolute slope function setting value
     * @param      rawref: Current setting value
     * @param      targetref: Target set point
     * @retval     Absolute value slope function setting value. If slope function is not enabled, the target setting
     * value is returned
     */
    float calcAbsRef(float rawref, float targetref) const {
        if (acc_ == 0.0f || dec_ == 0.0f) {
            return targetref;
        }

        if (rawref > 0.0f) {
            if (rawref < targetref - acc_) {
                return rawref + acc_;
            }
            if (rawref > targetref + dec_) {
                return rawref - dec_;
            }
            return targetref;
        }
        if (rawref > targetref + acc_) {
            return rawref - acc_;
        }
        if (rawref < targetref - dec_) {
            return rawref + dec_;
        }
        return targetref;
    }
};
}  // namespace os::math